#include "seer_command_impl.h"
#include <QJsonDocument>
#include <QJsonObject>
#include <QJsonArray>
#include <QUuid>
#include <QHostAddress>

namespace infrastructure {
namespace vehicle {
namespace seer {

SeerCommandImpl::SeerCommandImpl(std::shared_ptr<kernel::ILogger> logger, QObject* parent)
    : QObject(parent)
    , m_logger(logger)
{
    if (m_logger) {
        m_logger->info("SeerCommandImpl initialized");
    }
}

SeerCommandImpl::~SeerCommandImpl()
{
    m_agvConnections.clear();
    if (m_logger) {
        m_logger->info("SeerCommandImpl destroyed");
    }
}

bool SeerCommandImpl::registerAgv(int agvNumber, const QString& ipAddress)
{
    if (ipAddress.isEmpty()) {
        if (m_logger) {
            m_logger->error(QString("Cannot register AGV %1 with empty IP address").arg(agvNumber));
        }
        return false;
    }

    AgvConnection conn;
    conn.ipAddress = ipAddress;

    // 创建 roboTask 连接到 AGV:19206（RobotTask API）
    conn.roboTask = std::make_unique<robokit::RobotTask>();
    if (!conn.roboTask->initTcpConnection(QHostAddress(ipAddress), SEER_ROBOT_TASK_PORT)) {
        if (m_logger) {
            m_logger->error(QString("Failed to connect robo_task to SEER AGV %1 at %2:%3")
                .arg(agvNumber).arg(ipAddress).arg(SEER_ROBOT_TASK_PORT));
        }
        return false;
    }

    // 创建 robotOther 连接到 AGV:19210（RobotOther API - 顶升等其他功能）
    conn.robotOther = std::make_unique<robokit::RobotOther>(this);
    if (!conn.robotOther->initTcpConnection(QHostAddress(ipAddress), SEER_ROBOT_OTHER_PORT)) {
        if (m_logger) {
            m_logger->error(QString("Failed to connect robot_other to SEER AGV %1 at %2:%3")
                .arg(agvNumber).arg(ipAddress).arg(SEER_ROBOT_OTHER_PORT));
        }
        return false;
    }

    // 连接 RobotOther 的信号
    connect(conn.robotOther.get(), &robokit::RobotOther::jackLoadCompleted,
            this, [this, agvNumber](int ret_code, const QString& err_msg, const QString& create_on) {
                if (m_logger) {
                    if (ret_code == 0) {
                        m_logger->info(QString("SEER AGV %1 jack load completed successfully").arg(agvNumber));
                    } else {
                        m_logger->error(QString("SEER AGV %1 jack load failed: %2").arg(agvNumber).arg(err_msg));
                    }
                }
            });

    connect(conn.robotOther.get(), &robokit::RobotOther::jackUnloadCompleted,
            this, [this, agvNumber](int ret_code, const QString& err_msg, const QString& create_on) {
                if (m_logger) {
                    if (ret_code == 0) {
                        m_logger->info(QString("SEER AGV %1 jack unload completed successfully").arg(agvNumber));
                    } else {
                        m_logger->error(QString("SEER AGV %1 jack unload failed: %2").arg(agvNumber).arg(err_msg));
                    }
                }
            });

    m_agvConnections[agvNumber] = std::move(conn);

    if (m_logger) {
        m_logger->info(QString("SEER AGV %1 all connections established (IP: %2, Ports: %3/%4/%5)")
            .arg(agvNumber)
            .arg(ipAddress)
            .arg(SEER_COMMAND_PORT)
            .arg(SEER_ROBOT_TASK_PORT)
            .arg(SEER_ROBOT_OTHER_PORT));
    }

    return true;
}

void SeerCommandImpl::unregisterAgv(int agvNumber)
{
    auto it = m_agvConnections.find(agvNumber);
    if (it != m_agvConnections.end()) {
        m_agvConnections.erase(it);
        if (m_logger) {
            m_logger->info(QString("SEER AGV %1 command connections removed").arg(agvNumber));
        }
    }
}

SeerCommandImpl::AgvConnection* SeerCommandImpl::getConnection(int agvNumber)
{
    auto it = m_agvConnections.find(agvNumber);
    if (it == m_agvConnections.end()) {
        if (m_logger) {
            m_logger->warning(QString("No connection found for SEER AGV %1").arg(agvNumber));
        }
        return nullptr;
    }
    return &it->second;
}

// ============================================================================
// 命令实现（纯发送逻辑，参考 agv.cpp）
// 业务逻辑（路径比对、状态检查等）应在 VehicleService 中实现
// ============================================================================

/**
 * @brief 移动到站点列表
 * @details 参考 agv.cpp:229-273 的 JSON 构造部分
 */
kernel::Result<void> SeerCommandImpl::moveToStationList(
    int agvNumber,
    const std::vector<std::string>& stations,
    const std::string& operation)
{
    auto conn = getConnection(agvNumber);
    if (!conn) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "SEER AGV connection not found"
        );
    }

    if (stations.size() < 2) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::InvalidParameter,
            "Station list size must be at least 2"
        );
    }

    // 构建 JSON（参考 agv.cpp:229-259）
    QJsonObject rootObject;
    QJsonArray moveTaskListArray;

    for (size_t i = 0; i < stations.size() - 1; ++i) {
        QString sourceId = QString::fromStdString(stations[i]);
        QString targetId = QString::fromStdString(stations[i + 1]);

        QJsonObject moveTaskObj;
        moveTaskObj.insert("id", targetId);
        moveTaskObj.insert("source_id", sourceId);
        moveTaskObj.insert("task_id", QUuid::createUuid().toString().remove("{").remove("}").remove("-"));

        // Add operation to the last task if provided
        if (!operation.empty() && i == stations.size() - 2) {
            auto opString = QString::fromStdString(operation);
            moveTaskObj.insert("operation", opString);
            if (opString == QStringLiteral("JackLoad")) {
                moveTaskObj.insert("recognize", true);
                moveTaskObj.insert("recfile", QString("shelf/s0002.shelf"));
            }
        }

        moveTaskListArray.append(moveTaskObj);
    }

    rootObject.insert("move_task_list", moveTaskListArray);

    QJsonDocument jsDoc(rootObject);
    QByteArray jsonBuffer = jsDoc.toJson();

    // 使用 roboTask API 发送命令
    conn->roboTask->robot_task_gotargetlist_req_get(rootObject);

    if (m_logger) {
        m_logger->info(QString("===== Dispatching Command to AGV %1 =====").arg(agvNumber));
        m_logger->info(QString("SEER AGV %1 move task list: %2 stations%3")
            .arg(agvNumber)
            .arg(stations.size())
            .arg(operation.empty() ? "" : QString(", operation: %1").arg(QString::fromStdString(operation))));

        // Print detailed station list
        for (size_t i = 0; i < stations.size(); ++i) {
            QString station = QString::fromStdString(stations[i]);
            if (i == 0) {
                m_logger->info(QString("  Start: %1").arg(station));
            } else if (i == stations.size() - 1) {
                m_logger->info(QString("  Goal:  %1%2")
                    .arg(station)
                    .arg(operation.empty() ? "" : QString(" (with operation: %1)").arg(QString::fromStdString(operation))));
            } else {
                m_logger->info(QString("  Via:   %1").arg(station));
            }
        }
        m_logger->info(QString("===== End of Command ====="));
    }

    return kernel::Result<void>::success();
}

/**
 * @brief 移动到单个站点
 * @details 参考 agv.cpp:276-288 PublishMoveTask()
 */
kernel::Result<void> SeerCommandImpl::moveToStation(
    int agvNumber, const std::string& sourceId, const std::string& targetId)
{
    auto conn = getConnection(agvNumber);
    if (!conn) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "SEER AGV connection not found"
        );
    }

    // 构建 JSON（参考 agv.cpp:279-284）
    QJsonObject rootObject;
    rootObject.insert("id", QString::fromStdString(targetId));
    rootObject.insert("source_id", QString::fromStdString(sourceId));

    // 使用 roboTask API 发送命令
    conn->roboTask->robot_task_gotarget_req_get(rootObject);

    if (m_logger) {
        m_logger->info(QString("SEER AGV %1 move task: %2 -> %3")
            .arg(agvNumber)
            .arg(QString::fromStdString(sourceId))
            .arg(QString::fromStdString(targetId)));
    }

    return kernel::Result<void>::success();
}

/**
 * @brief 取消任务
 * @details 参考 agv.cpp:334-344 ClearTask()
 */
kernel::Result<void> SeerCommandImpl::cancelTask(int agvNumber)
{
    auto conn = getConnection(agvNumber);
    if (!conn) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "SEER AGV connection not found"
        );
    }

    // 使用 roboTask API 发送取消命令
    conn->roboTask->robot_task_cancel_req_get();

    if (m_logger) {
        m_logger->info(QString("SEER AGV %1 task canceled").arg(agvNumber));
    }

    return kernel::Result<void>::success();
}

/**
 * @brief 暂停任务
 * @details 参考 agv.cpp:418-424 pauseTask()
 */
kernel::Result<void> SeerCommandImpl::pauseTask(int agvNumber)
{
    auto conn = getConnection(agvNumber);
    if (!conn) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "SEER AGV connection not found"
        );
    }

    // 使用 roboTask API 发送暂停命令
    conn->roboTask->robot_task_pause_req_get();

    if (m_logger) {
        m_logger->info(QString("SEER AGV %1 task paused").arg(agvNumber));
    }

    return kernel::Result<void>::success();
}

/**
 * @brief 恢复任务
 * @details 参考 agv.cpp:425-431 resumeTask()
 */
kernel::Result<void> SeerCommandImpl::resumeTask(int agvNumber)
{
    auto conn = getConnection(agvNumber);
    if (!conn) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "SEER AGV connection not found"
        );
    }

    // 使用 roboTask API 发送恢复命令
    conn->roboTask->robot_task_resume_req_get();

    if (m_logger) {
        m_logger->info(QString("SEER AGV %1 task resumed").arg(agvNumber));
    }

    return kernel::Result<void>::success();
}

/**
 * @brief 控制顶升
 * @details 使用 RobotOther API (19210端口) 的顶升控制
 */
kernel::Result<void> SeerCommandImpl::controlJack(int agvNumber, bool raise)
{
    auto conn = getConnection(agvNumber);
    if (!conn) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "SEER AGV connection not found"
        );
    }

    if (!conn->robotOther) {
        if (m_logger) {
            m_logger->error(QString("SEER AGV %1 RobotOther not initialized").arg(agvNumber));
        }
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "RobotOther API not initialized"
        );
    }

    // 使用 RobotOther API 发送顶升命令
    if (raise) {
        conn->robotOther->robot_other_jack_load_req();  // 6070 - 顶升上升
        if (m_logger) {
            m_logger->info(QString("SEER AGV %1 jack load request sent via RobotOther API").arg(agvNumber));
        }
    } else {
        conn->robotOther->robot_other_jack_unload_req();  // 6071 - 顶升下降
        if (m_logger) {
            m_logger->info(QString("SEER AGV %1 jack unload request sent via RobotOther API").arg(agvNumber));
        }
    }

    return kernel::Result<void>::success();
}

/**
 * @brief 开始充电
 * @details MKLM 特有功能，SEER 暂不支持
 */
kernel::Result<void> SeerCommandImpl::startCharging(int agvNumber, const std::string& chargePoint)
{
    // TODO: SEER AGV 充电功能待实现（MKLM特有功能）
    if (m_logger) {
        m_logger->warning(QString("SEER AGV %1 start charging not implemented").arg(agvNumber));
    }
    return kernel::Result<void>::failure(
        kernel::ErrorCode::NotImplemented,
        "Start charging not supported for SEER AGV"
    );
}

/**
 * @brief 停止充电
 * @details MKLM 特有功能，SEER 暂不支持
 */
kernel::Result<void> SeerCommandImpl::stopCharging(int agvNumber)
{
    // TODO: SEER AGV 充电功能待实现（MKLM特有功能）
    if (m_logger) {
        m_logger->warning(QString("SEER AGV %1 stop charging not implemented").arg(agvNumber));
    }
    return kernel::Result<void>::failure(
        kernel::ErrorCode::NotImplemented,
        "Stop charging not supported for SEER AGV"
    );
}

/**
 * @brief 控制门
 * @details MKLM 特有功能，SEER 暂不支持
 */
kernel::Result<void> SeerCommandImpl::controlDoor(int agvNumber, int doorId, bool open)
{
    // TODO: SEER AGV 门控制功能待实现（MKLM特有功能）
    if (m_logger) {
        m_logger->warning(QString("SEER AGV %1 door control not implemented").arg(agvNumber));
    }
    return kernel::Result<void>::failure(
        kernel::ErrorCode::NotImplemented,
        "Door control not supported for SEER AGV"
    );
}

/**
 * @brief 请求当前地图
 * @details MKLM 特有功能，SEER 暂不支持
 */
kernel::Result<void> SeerCommandImpl::requestCurrentMap(int agvNumber)
{
    // TODO: SEER AGV 地图请求功能待实现（MKLM特有功能）
    if (m_logger) {
        m_logger->warning(QString("SEER AGV %1 request map not implemented").arg(agvNumber));
    }
    return kernel::Result<void>::failure(
        kernel::ErrorCode::NotImplemented,
        "Request current map not supported for SEER AGV"
    );
}

kernel::Result<void> SeerCommandImpl::switchMap(
    int agvNumber,
    const std::string& mapName,
    const std::string& switchPoint)
{
    auto conn = getConnection(agvNumber);
    if (!conn || !conn->roboTask) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            "SEER AGV connection not found");
    }

    QJsonObject rootObject;
    rootObject.insert("operation", QStringLiteral("Script"));
    rootObject.insert("script_name", QStringLiteral("syspy/switchMap.py"));
    rootObject.insert("source_id", QStringLiteral("SELF_POSITION"));
    rootObject.insert("id", QStringLiteral("SELF_POSITION"));
    rootObject.insert("task_id", QUuid::createUuid().toString(QUuid::WithoutBraces));

    QJsonObject argsObject;
    argsObject.insert("map", QString::fromStdString(mapName));
    argsObject.insert("switchPoint", QString::fromStdString(switchPoint));
    rootObject.insert("script_args", argsObject);

    conn->roboTask->robot_task_gotarget_req_get(rootObject);

    if (m_logger) {
        m_logger->info(QString("SEER AGV %1 switch map -> %2 (point %3)")
                           .arg(agvNumber)
                           .arg(QString::fromStdString(mapName))
                           .arg(QString::fromStdString(switchPoint)));
    }

    return kernel::Result<void>::success();
}

} // namespace seer
} // namespace vehicle
} // namespace infrastructure
